#coding: utf-8
from peanut_app import PeanutAPP
from peanut_ros import PeanutROS
import uiautomator2 as u2
import time
import re
import logging
import sys
import os
import atexit
import traceback
from roslibpy import Ros
from flask import Flask, request
import signal

ANDROID_IP_ADDRESS = '192.168.100.22'
ROS_MASTER_IP_ADDRESS = '192.168.100.21'
CENTER_IP_ADDRESS = '192.168.100.10'


class Peanut:
    def __init__(self, app: PeanutAPP, ros: PeanutROS):
        # logger
        LOG_FORMAT = "%(asctime)s[PEANUT]%(message)s"
        DATE_FORMAT = "[%Y/%m/%d %H:%M:%S]"

        self.logger = logging.getLogger('peanut')
        self.logger.setLevel(logging.INFO)
        formatter = logging.Formatter(fmt=LOG_FORMAT, datefmt=DATE_FORMAT)
        handler = logging.StreamHandler(sys.stdout)
        handler.setFormatter(formatter)
        self.logger.addHandler(handler)

        self.app = app
        self.ros = ros
        self.center = Ros(CENTER_IP_ADDRESS, 9090)
        self.center.run()
        if not self.center.is_connected:
            self.logger.error("无法连接到中心系统")
            sys.exit(-1)
        else:
            self.logger.info("连接到中心系统")

        self.stop_charge()
        self.in_charge_process = False

        # 获取目标点列表、目标点名称列表
        self.targets = []
        self.target_dict = {}
        self.update_targets()

        self.in_cabin = False

        # target related parameters
        self.current_target = {
            'target_name': "",
            'phone_number': "17700000000"
        }

        # 标记花生是否在移动
        self.on_moving = False
        # 标记当前是否有订单
        self.order_ready = False 

        # 简单的手机号判断
        self.phone_re = re.compile(r"^[1][3-9][0-9]{9}$")
        
        # 记录送餐之前的托盘重量
        self.init_weight = self.ros.cur_weighing

        # watchers
        # self.app.device.watcher('RETURN').when('返回服务台').call(
        #     lambda : self.into_cabin() 
        # )
        # self.app.device.watcher('OPEN_HATCH').when('开门中，请稍等…').call(
        #     lambda : self.app.set_hatch_status(True)
        # )
        # self.app.device.watcher('CLOSE_HATCH').when('关门中，请稍等…').call(
        #     lambda : self.app.set_hatch_status(False)
        # )
        # self.app.device.watcher('FETCH').when('开门取物').call(self.app.open_hatch_on_delivered)

        # watchContext
        # self.ctx = self.app.device.watch_context()

        # for sig in [signal.SIGTERM, signal.SIGINT, signal.SIGHUP, signal.SIGQUIT]:
        #     signal.signal(sig, self.signal_handler)
        #     signal.siginterrupt(sig, False)

        self.logger.info('Peanut 初始化完成')

    def update_targets(self):
        """更新目标点数组
        """
        self.targets = self.ros.get_target_points()
        for t in self.targets:
            self.target_dict[t['name']] = t['pose']

    def delivery(self, target_name: str, phone_number: str=None, pose: dict = None):
        """通过 APP 调用送物

        Args:
            target_name (str): 目标名
            phone_number (str): 手机号
            pose (dict, optional): 如果目标是"送餐点"，传入这个参数以动态改变送餐位置
        """
        if target_name not in self.target_dict.keys():
            self.logger.error('非法目标点')
            return
        if phone_number:
            if not self.phone_re.match(phone_number):
                # 手机号非法
                self.logger.error('非法手机号')
                return
        else:
            # 合法目标点和合法手机号，执行送物流程
            self.current_target['target_name'] = target_name
            self.current_target['phone_number'] = phone_number
            self.stop_charge()
            # 动态自定义目标点
            if target_name == '送餐点' and pose:
                self.modify_dynamic_target(pose)
                self.update_targets()
            # 通过 app 模拟点击执行送物流程
            self.app.delivery(target_name=target_name, phone_number=phone_number, wait=False)

    def dispatch(self, target_name: str, pose: dict = None):
        """通过 APP 执行派遣

        Args:
            target_name (str): 目标名
            pose (dict, optional): 如果 target_name 是"送餐点"，传入这个参数以动态改变送餐位置
        """
        if target_name not in self.target_dict.keys():
            self.logger.error('非法目标点')
            return
        else:
            # 合法目标点，执行派遣流程
            self.stop_charge()
            # 动态自定义目标点
            if target_name == '送餐点' and pose:
                self.modify_dynamic_target(pose)
                self.update_targets()
            # 通过 app 模拟点击执行派遣流程
            self.app.dispatch(target_name=target_name)

    def modify_dynamic_target(self, pose):
        # @TODO: NOT USED in current version
        """临时修改送餐点

        Args:
            pose (geometry_msgs/Pose): 送餐位置的坐标，格式是 geometry_msgs/Pose
        """
        target = [target for target in self.targets if target['name'] == '送餐点'][0]
        self.ros.delete_target_point(target)
        target['pose'] = pose
        self.ros.insert_target_point(target)
        self.logger.info('Modify dynamic target pose to: {}'.format(pose))
    
    def charge(self):
        """调用 app 命令机器人返回充电
        """
        if self.in_charge_process:
            return
        self.in_charge_process = True
        if self.in_cabin:
            out = self.out_cabin()
        else:
            out = True
        if out:
            self.app.charge()
        else:
            self.logger.error('机器人在舱体内, 无法返回充电.')
        self.in_charge_process = False

    def stop_charge(self):
        """调用 APP 和 ROS 停止机器人充电
        """
        charge_stopped = self.app.stop_charge()
        if not charge_stopped:
            self.ros.stop_charge()

    def start_watch(self, use_watcher=True):
        if use_watcher:
            self.app.device.watcher.start()
        else:
            self.ctx = self.app.device.watch_context()
            self.ctx.when('开门中，请稍等…').call(
                lambda : self.app.set_hatch_status(True)
            )
            self.ctx.when('关门中，请稍等…').call(
                lambda : self.app.set_hatch_status(False)
            )
            self.ctx.when('开门取物').call(self.app.open_hatch_on_delivered)

    def stop_watch(self, use_watcher=True):
        if use_watcher:
            self.app.device.watcher.stop()
        else:
            self.ctx.close()

    def into_cabin(self):
        """控制机器人进入舱体

        Returns:
            bool: 是否顺利进入舱体
        """
        # 导航到舱体前
        if self.in_cabin:
            return True
        at_cabin1 = self.stand_by()
        if not at_cabin1:
            self.logger.error('无法到达舱体前')
            self.in_cabin = False
            return False

        # 进入舱体
        self.ros.move_to(self.target_dict['cabin2'])
        for _ in range(60):
            if self.ros.goal_reached:
                self.logger.info('进入舱体内部')
                break
            else:
                time.sleep(1)

        if self.ros.goal_reached:
            self.ros.goal_reached = False
            time.sleep(1)
        else:
            self.logger.error('无法进入舱体内部')
            self.in_cabin = False
            return False

        # 前进 15 厘米
        self.ros.move_by(dis=0.2, linear_speed=0.05)
        self.logger.info('到达取咖啡位置')
        time.sleep(1)
        self.in_cabin = True

        # 打开 APP 主界面
        self.app.launch_delivery_main_menu_page()
        return True

    def out_cabin(self, stand_by=False):
        """控制机器人离开舱体到舱体前的位置待机

        Returns:
            bool: True 表示顺利到达待机位置，否则为 False
        """
        self.ros.move_by(dis=-0.2, linear_speed=0.05)
        self.ros.move_by(angle=-90, angular_speed=0.5)
        self.ros.move_by(dis=-0.2, linear_speed=0.05)
        self.ros.move_by(angle=180, angular_speed=0.5)
        self.in_cabin = False
        if stand_by:
            return self.stand_by()
        else:
            self.logger.info('花生离开舱体')
            
            return True

    def stand_by(self, retry=10):
        self.stop_charge()
        if self.in_cabin:
            self.out_cabin()
        # self.app.launch_delivery_main_menu_page()
        for _ in range(retry):
            self.ros.move_to(self.target_dict['cabin1'])
            for _ in range(30):
                if self.ros.goal_reached:
                    self.logger.info('花生到达待机位置')

                    return True
                else:
                    time.sleep(3)
            self.logger.error('花生无法到达待机位置, 重试中...')
        self.logger.error('花生无法到达待机位置, 执行返回充电...')
        self.charge()
        return False

    def move(self, dis, speed):
        """move forward via ros

        Args:
            dis (float): 前进距离, 单位米.负数表示倒退.
            speed (float): 线速度, 单位 m/s.
        """
        self.ros.move_by(dis=dis, linear_speed=speed)

    def rotate(self, angle, speed):
        """rotate via ros

        Args:
            angle (float): 旋转角度, 单位为度数. 正数为逆时针旋转, 负数为顺时针旋转.
            speed (float): 角速度, 单位 rad/s.
        """

        self.ros.move_by(angle=angle, angular_speed=speed)

    def move_to(self, target_name):
        """使用 ROS 移动到指定的 target

        Args:
            target_name (string): target name, 也就是 self.target_dict 的 key
        """
        if target_name in self.target_dict.keys():
            self.ros.move_to(self.target_dict[target_name])
        else:
            self.logger.error('非法目标点')

    def stop_move(self):
        """stop robot to move via ros
        """
        self.ros.stop_move()

    def open_hatch(self):
        """open hatch via app
        """
        self.app.open_hatch()

    def close_hatch(self):
        """close hatch via app
        """
        self.app.close_hatch()

    def signal_handler(self, sig, frame):
        print('Ctrl-C triggered.')
        self.charge()
        sys.exit(0)


    def move_into_cabin(self):
        self.stop_move()
        self.stop_charge()

        if not self.in_cabin:
            self.into_cabin()
        # 记录初始重量
        self.init_weight = self.ros.cur_weighing
        self.center.set_param('/peanut/ready', 'yes')  # 告知机械臂花生已就位
        self.logger.info('[SEND SIGNAL] /peanut/ready, \'yes\' ')

    def open__hatch(self, use_app=True):
        if self.in_cabin:
            if use_app:
                self.app.delivery_wait()
            else:
                self.app.open_hatch()
            # 若机械臂未放置好咖啡，点击屏幕保持上盖开启
            # 最多等待 300 秒，否则直接离开
            timeout = 420
            while self.center.get_param('/peanut/coffee_place') in ['no', None] and timeout > 0:
                self.app.device.click(0, 0)
                timeout -= 3
                time.sleep(3)
                print(timeout)
        else:
            self.logger.error("无法取到咖啡.")
            return False
    '''
    def wait_cup(self):
            # 若机械臂未放置好咖啡，点击屏幕保持上盖开启
            # 最多等待 300 秒，否则直接离开
            timeout = 300
            while self.center.get_param('/peanut/coffee_place') in ['no', None] and timeout > 0:
                self.app.device.click(0, 0)
                timeout -= 3
                time.sleep(3)
                print(timeout)
            # self.logger.info('[GET SIGNAL] /peanut/coffee_place, \'yes\' ')
    '''

    def deliver_coffee(self, target_name=None, phone_number=None, use_app=True):
        '''
        if target_name is None:  # 等待中心系统告知桌号
            target_name = str(self.center.get_param('/peanut/table_id'))
        assert target_name is not None, "Target name is None."
        # 多目标用 '_' 分隔, 先到的订单放后面(因为要 pop)
        splited_target_name = target_name.split('_')[::-1]
        # 如果多杯是同一桌, 去掉重复桌号
        self.current_target['target_name'] = sorted(list(set(splited_target_name)),
                                                    key=splited_target_name.index)
        '''
        self.logger.info('目标桌号: %s' % target_name)

        self.center.set_param('/peanut/ready', 'no')  # 告知机械臂花生离开
        self.logger.info('[SEND SIGNAL] /peanut/ready, \'no\' ')
        if use_app:
            self.out_cabin(stand_by=False)
            self.center.set_param('/peanut/coffee_place', 'no')  # 重置机械臂咖啡放置标记
            self.logger.info('[SEND SIGNAL] /peanut/coffee_place, \'no\' ')
        else:
            self.out_cabin(stand_by=False)
            self.center.set_param('/peanut/coffee_place', 'no')  # 重置机械臂咖啡放置标记
            self.logger.info('[SEND SIGNAL] /peanut/coffee_place, \'no\' ')

        #while len(self.current_target['target_name']) > 0:
        # 先来的订单在列表的末尾
        target = target_name + "号桌"

        if use_app:
            self.app.delivery_input(target, phone_number)
        self.logger.info('咖啡送往' + target)

        weight_before_taken = self.ros.cur_weighing
        self.logger.info('咖啡重量: %.2f' % (weight_before_taken / 10))

        self.in_cabin = False
        self.stop_move()  # 重置 ros.goal_reach

        if use_app:
            self.app.delivery_go()
        else:
            self.ros.move_to(self.target_dict[target_name])

        for _ in range(90):  # 等待到达目标
            if not self.ros.goal_reached:
                time.sleep(1)

        if self.ros.goal_reached:
            self.logger.info('咖啡送达' + target)
            if use_app:
                self.app.open_hatch_on_delivered(phone_number)  # 打开舱门
                self.app.device(textContains="关门返回").wait(10)  # 等待 关门返回 按钮被按下
            else:
                self.app.open_hatch()
            self.logger.info('舱门已打开')
            # 每隔三秒检测重量：
            for _ in range(15):
                if len(self.current_target['target_name']) > 0:
                    # 如果有多桌, 拿走一杯就离开, 一个空杯子大约是 7 克, 这里设置 6 克的阈值
                    coffee_taken = weight_before_taken - self.ros.cur_weighing > 60
                    weight_before_taken = self.ros.cur_weighing
                else:
                    # 如果只有一桌, 全部拿走再离开, 误差 2 克
                    coffee_taken = abs(self.ros.cur_weighing - self.init_weight) < 20
                if coffee_taken:
                    # 咖啡被取走
                    self.logger.info("咖啡被取走")
                    time.sleep(4)
                    if use_app:
                        self.app.device(textContains="关门返回").click_exists()
                    else:
                        self.close_hatch()
                    self.logger.info('舱门已关闭')
                    break
                elif use_app and not self.app.device(textContains="关门返回").exists():
                    # 咖啡(可能)没有被取走, 但是舱门已经被关闭
                    self.logger.info("舱门已关闭")
                    break
                else:
                    time.sleep(3)
            '''
            # 送多杯, 退回到送餐界面修改目标点. 如果是用 ROS 走的流程直接重新进入循环就可以
            if len(self.current_target['target_name']) > 0:
                if use_app:
                    self.app.device.wait_activity(self.app.activities['导航'], timeout=10.0)
                    self.app.device.press('back')
                continue
            '''
        else:
            self.logger.error('无法到达 %s' % target)
            return False



device = None
peanut = None
try:
    os.system('adb kill-server')
    os.system('adb start-server')
    os.system('adb connect ' + ANDROID_IP_ADDRESS)
    device = u2.connect_adb_wifi(ANDROID_IP_ADDRESS+':5555')
except Exception:
    print("uiautomator2 无法连接.")
    exit(-1)
if device is not None:
    app = PeanutAPP(device)
    ros = PeanutROS(master_url=ROS_MASTER_IP_ADDRESS)
    peanut = Peanut(app, ros)

# === 注册 atexit 处理器, 请注意两个函数的顺序不能改变 ===
@atexit.register
def disconnect_adb():
    os.system('adb disconnect')
    os.system('adb kill-server')


@atexit.register
def charge_on_exit():
    peanut.charge()


#启用Flask来进行通信
app = Flask(__name__)
center = Ros(CENTER_IP_ADDRESS, 9090)
center.run()
        
@app.route('/')
def hello():
    return 'hello'

@app.route('/move_tocabin/',methods=['POST'])
def peanut_MoveIntoCabin():
    peanut.move_into_cabin()
    return 'Success'

@app.route('/open_hatch/',methods=['POST'])
def peanut_open_hatch():
    peanut.open__hatch(use_app=True)
    return 'Open_hatch Success'
'''
@app.route('/peanut_WaitCup/',methods=['POST'])
def peanut_WaitCup():
    peanut.wait_cup()
    return 'Success'
'''
@app.route('/close_hatch/',methods=['POST'])
def peanut_close_hatch():
    print("!!!!!!close_hatch!!!!!!")
    peanut.close_hatch()
    return 'Success'

@app.route('/deliver_item/',methods=['POST'])
def peanut_DeliverCoffee():
    target_name = request.values['table_id']
    peanut.deliver_coffee(target_name,use_app=True)
    return 'Success'

@app.route('/peanut_Charge/',methods=['POST'])
def peanut_Charge():
    peanut.charge()
    return 'Success'


if __name__ == '__main__':
    app.run(host='0.0.0.0' ,port=5002)